/***************************************************************************
 *            grab_dv.cpp
 *
 *  Thu Apr 30 10:25:58 2009
 *  Copyright  2010  Michael Warren
 *  Queensland University of Technology, CyPhy Lab
 *  <michael.warren@qut.edu.au>
 ****************************************************************************/

// OpenCV
#include "cv.h"
#include "highgui.h"

#include <dc1394cam_driver/dc1394cam_driver.h>

#include <vector>
#include <string>
#include <time.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <iostream>
#include <cstdlib>
#include <iomanip>
#include <signal.h>
#include <map>
#include <sys/time.h>

//Setting process priority
#include <sys/time.h>
#include <sys/resource.h>

// VXL
#include <vul/vul_arg.h>
#include <vil/vil_save.h>

#define VIDERE 111
#define PTGREY 222

int camera = PTGREY;

//using namespace std;


// Global variables
int vflag = 1;
int num_frames = 0;
int write_format = 0;
int i;
static char time_string[40];
CvVideoWriter *writer1 = 0;


// Create camera capture
multiCamCapture* captures_ptr;

// Create video writers
std::vector<CvVideoWriter*> *writers_ptr;

// Helper Functions

void quit(multiCamCapture *captures, std::vector<CvVideoWriter*> writers) {
    if (vflag) 
      cvDestroyWindow( "Output");
    captures->close();
    for (i = 0; i <writers.size(); i++) { 
        if (writers[i]) {
            std::cout << "releasing video writer " << i << "." << std::endl;
            cvReleaseVideoWriter(&writers[i]);
        }
    }
    exit(0);
}

bool isItColor(std::string format) {
    bool result = false;
    if (format == "RGB" )
        result = true;
    else if (format == "YUV")
        result = true;
    else if (format == "GREY")
        result = false;
    else if (format == "RGB8")
        result = true;  
    else if (format == "RGB16")
        result = true;
    else if (format == "YUV411")
        result = false;
    else if (format == "YUV422")
        result = false;
    else if (format == "YUV444")
        result = false;
    else if (format == "GREY8")
        result = false;
    else if (format == "GREY16")
        result = false;
    else if (format == "RAW8")
        result = false;
    else if (format == "RAW16")
        result = false;
    else {
      std::cerr << "Unidentified format " << format << "!" << std::endl;
      exit(-1);
    }
    return result;
}

void 
cvtToBgr( const IplImage* src, IplImage* dest, const std::string format )
{
    if( format == "BGR8" )
    {
        // do nothing but copy data into destination array.
        // Need to do the copy just in case the src image is being manipulated
        // and we don't want those manipulations to occur on the destination image
        // as well
        cvCopy(src, dest);

    }
    else if( format == "RGB8" )
    {
        cvCvtColor( src, dest, CV_RGB2BGR );
    }
    else if( format == "BGRA8" )
    {
        cvCvtColor( src, dest, CV_BGRA2BGR );
    }
    else if( format == "RGBA8" )
    {
        cvCvtColor( src, dest, CV_BGRA2BGR );
    }
    else if( format == "BayerRG8" )
    {
        cvCvtColor( src, dest, CV_BayerRG2BGR );
    }
    else if( format == "BayerGR8" )
    {
        cvCvtColor( src, dest, CV_BayerGR2BGR );
    }
    else if( format == "BayerGB8" )
    {
        cvCvtColor( src, dest, CV_BayerGB2BGR );
    }
    else if( format == "BayerBG8" )
    {
        cvCvtColor( src, dest, CV_BayerBG2BGR );
    }
    else if( format == "GRAY8" )
    {
        cvCvtColor( src, dest, CV_GRAY2BGR );
    }
    else
    {
        std::cout << "ERROR(colourconversions.cpp): Don't know how to convert image format into BGR8." << std::endl;
        //TODO: throw an exception
        exit(1);
    }
}

void 
cvtToGrey( const IplImage* src, IplImage* dest, const std::string format )
{
    if( format == "GREY8" )
    {
        // do nothing but copy data into destination array.
        // Need to do the copy just in case the src image is being manipulated
        // and we don't want those manipulations to occur on the destination image
        // as well
        cvCopy(src, dest);

    }
    else if( format == "RGB8" )
    {
        cvCvtColor( src, dest, CV_RGB2GRAY );
    }
    else if( format == "BGRA8" )
    {
        cvCvtColor( src, dest, CV_BGRA2GRAY );
    }
    else if( format == "RGBA8" )
    {
        cvCvtColor( src, dest, CV_BGRA2GRAY );
    }
    else if( format == "BGR8" )
    {
        cvCvtColor( src, dest, CV_BGR2GRAY );
    }
    else
    {
        std::cout << "ERROR(colourconversions.cpp): Don't know how to convert image format into GRAY8." << std::endl;
        //TODO: throw an exception
        exit(1);
    }

}



int makeDirCurrTime(char* out_string) {
    time_t curr_time;
    curr_time = time(NULL);
    struct tm *formatted_time;
    formatted_time = localtime(&curr_time);

    size_t len = strftime(out_string, 40, "%y_%m_%d_%H_%M_%S",formatted_time);
    if (!len) return -1;
    // Make dir with RWX for owner/group RX for others
    if (mkdir(out_string, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))
        return -1;
    printf("Getting time...\n");
    if(chdir(out_string)) 
        return -1;
    if(chdir("..")) 
        return -1;    
    printf("Made directory: %s\n", out_string);
    return 0;
}

void sighandler(int sig)
{
    std::cout << "caught signal!" << std::endl;
    quit(captures_ptr, *writers_ptr);
}

int main(int argc, char* argv[]) {

    // signal handlers
    //signal(SIGABRT, &sighandler);
	  //signal(SIGTERM, &sighandler);
	  signal(SIGINT, &sighandler);

    // Attempt to make this process high priority if permissions allow
    std::cout << "Attempting to set high priority... ";
    if (setpriority(PRIO_PROCESS,0,-20))
      std::cout << "could not." << std::endl;
    else 
      std::cout << "could!" << std::endl;

    // Generic settings
    vul_arg<int> in_frames("-num_frames", "number_of_frames_to_capture", 0);

    // Camera and firewire settings
    vul_arg<unsigned> in_num("-n", "number of cameras", 1);
    vul_arg<vcl_string> in_framerate("-f", "camera frame rate", "7.5");
    vul_arg<vcl_string> in_format("-in_format", "colour, grey, bayer etc. e.g. RGB, GREY, YUV", "RGB");
    vul_arg<bool> in_cleanup("-cleanup", "attempt to cleanup firewire bus", false);
    vul_arg<unsigned> in_width("-width", "desired width of captured images", 640);
    vul_arg<unsigned> in_height("-height", "desired height of captured images", 480);
    vul_arg<unsigned> in_bufs("-num_bufs", "number of dc1394 driver bufs", 16);
    vul_arg<vcl_string> in_guid("-guid_string", "assign specific cameras to gui indexes. e.g. -guid_string 0:49712223530833906,1:49712223530833870", "");
    vul_arg<unsigned> in_speed("-iso_speed", "set iso speed: 400, 800 etc", 400);
    vul_arg<unsigned> max_shutter("-max_shutter", "set the pt grey max shutter value", 0);
    vul_arg<float> max_shutter_abs("-max_shutter_abs", "set the pt grey max shutter value", 0);

    // Visualisation
    vul_arg<bool> in_vertical("-vertical", "vertical or horizontal stereo", false);
    vul_arg<bool> in_visualise("-suppress", "suppress visualisation", false);
    vul_arg<unsigned> in_win_width("-window_width", "desired width of visulisation images", 640);
    vul_arg<unsigned> in_win_height("-window_height", "desired height of visualisation images", 480);

    // Output settings
    vul_arg<vcl_string> in_out("-out", "output directory", ".");
    vul_arg<bool> in_output_video("-output_video", "output video flag", false);
    vul_arg<bool> in_output_combined_video("-output_combined_video", "output combined video flag", false);
    vul_arg<bool> in_output_image("-output_images", "output images flag", false);
    vul_arg<vcl_string> out_format("-out_format", "image output format", "jpg");
    vul_arg<bool> in_output_key("-output_on_key", "output images on 'c' key", false);
    vul_arg<bool> in_force_grey("-force_grey", "force the output images or videos to be greyscale", false);
    vul_arg<vcl_string> in_codec("-out_codec", "video output codec", "");
    vul_arg<bool> in_overwrite("-overwrite", "over-write images in a directory if it already exists", false);
    vul_arg<bool> in_single("-single", "Write video to a single file", false);

    // Parse arguments
    uint64_t guid;
    vul_arg_parse(argc, argv);

    // Set settings
    num_frames = in_frames();
    opterr = 0;

    // Camera and firewire settings
    int cleanup = in_cleanup();
    int height = in_height();
    int width = in_width();
    int number_of_cams = in_num();
    double framerate = 7.5;
    int isColor = isItColor(std::string(in_format()));
    int bufs = in_bufs();

    // Visulisation
    unsigned horiz_window_size = in_win_width();
    unsigned vert_window_size = in_win_height();
    vflag = in_visualise() ? 0 : 1;
    
    long unsigned int dataCounter_ = 1;
    
    
    // Create camera captures
    //cout << "Making captures..." << std::endl;
    multiCamCapture captures(number_of_cams);
    //cout << "Finished making captures..." << std::endl;
    captures_ptr = &captures;
    

    // Serial (GUID) numbers
    std::map<unsigned, uint64_t> serial_num;
    size_t search_pos = 0;
    std::string search_string = in_guid();
    //cout << "search std::string: " << search_string << std::endl;
    while (search_pos < search_string.size()) {
      size_t num_pos = search_string.find(':', search_pos);
      std::cout << "num_pos: " << num_pos << std::endl;
      std::string num_string;
      for (unsigned ii = search_pos; ii < num_pos; ii++) {
        num_string.append(1, search_string[ii]);
      }
      std::istringstream num_ss(num_string);
      int num;
      if (!(num_ss>>num)) {
       std::cout << "could not make an unsigned number from \"" << num_ss.str() << "\"!" << std::endl;
      }
      else {
        std::cout << "NUM: " << num << std::endl;
      }
      size_t id_pos = search_string.find(',', num_pos);
      if (id_pos == std::string::npos) {
        id_pos = search_string.size();
      }
      std::cout << "id_pos: " << id_pos << std::endl;
      std::string id_string;
      for (unsigned ii = num_pos+1; ii < id_pos; ii++) {
        id_string.append(1, search_string.at(ii));
      }
      std::istringstream id_ss(id_string);      
      uint64_t id;
      if (!(id_ss>>id)) {
       std::cout << "could not make a uint64_t from " << num_ss.str() << "!" << std::endl;
      }
      else {
        std::cout << "id: " << id << std::endl;
      }
      std::pair<unsigned, uint64_t> input_pair(num, id);
      serial_num.insert(input_pair);
      search_pos = id_pos+1;
    }


    // Frame rate
    std::istringstream convertStream;
    convertStream.str(std::string(in_framerate()));
    if(convertStream>>framerate)
      std::cout << "FPS: " << framerate << std::endl;
    else 
      std::cout << "could not set framerate!" << std::endl;

    //***************************** Output settings *********************************************
    std::string out_string((in_out().c_str()));
    // Create output directory if desired
    if ((in_output_key() || in_output_video() || in_output_combined_video() || in_output_image()) && !(out_string == ".")){
        // Make dir with RWX for owner/group RX for others
        if (mkdir(out_string.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH)) {
            if (in_overwrite()) {
                std::cout << "Warning: Over-writing files in output directory" << std::endl;
            } else {
                std::cout << "Could not create output directory" << std::endl;
                return -1;
            }
        }
        std::cout << "Output Directory: " << out_string << std::endl;
    }
    else if (in_output_key() || in_output_video() || in_output_combined_video() || in_output_image()) {
        if (makeDirCurrTime(time_string)) {
          printf("Could not create a directory!\n");
          exit(0);
        } else {
          out_string.assign(std::string("./") + time_string);
        }
    }
    
    // ************ Image Settings *******************************
    if (in_output_image() && (out_format() == "bmp" || out_format() == "jpg" 
                           || out_format() == "tif" || out_format() == "ppm" 
                           || out_format() == "pgm" )) {
        std::cout << "output image format: " << out_format() << std::endl;
    }
    else if (in_output_image()) {
        std::cout << "cannot determine image format: " << out_format() << ". Valid values are bmp, jpg or tif." << std::endl;
        exit(1);
    }
    
    
    // ************ Individual Video Settings *******************************
    // Create video writers
    std::vector<CvVideoWriter*> writers;
    writers_ptr = &writers;
    
    if (in_output_video()) {
        writers.resize(number_of_cams, NULL);
        printf("making videowriters...\n");
        for (int i = 0; i < writers.size(); i++) {
           std::stringstream filename;
            std::string codec = in_codec();
            if (!codec.size()) {
                filename << out_string.c_str() << "/" << "cam" << i << ".avi";
                std::string sfilename = filename.str();
                std::cout << "Creating video file at: " << sfilename.c_str() << std::endl;
                writers[i]=cvCreateVideoWriter(sfilename.c_str(),0x00000000,framerate,cvSize(width, height),isColor ? 1 : 0);
            } else if (codec.size() == 4) {
                filename << out_string.c_str() << "/" << "cam" << i << "_" << codec << ".avi";
                std::string sfilename = filename.str();
                std::cout << "Creating video file at: " << sfilename.c_str() << std::endl;
                char codec_char[] = {codec.at(0), codec.at(1), codec.at(2), codec.at(3)};
                writers[i]=cvCreateVideoWriter(sfilename.c_str(),CV_FOURCC(codec_char[0], codec_char[1], codec_char[2], codec_char[3]),framerate,cvSize(width, height),isColor ? 1 : 0);
            } else {
              std::cout << "Codec std::string not four characters!" << std::endl;
              assert(false);
            }
	        std::cout << "Creating videos with color? " << isColor << std::endl;
            if (!writers[i]) {
                printf("Could not create video file %d!\n", i);
                quit(&captures, writers);
            }
            if (in_single()) {
                break;
            }
        }
    }
    // ************ Combined Video Settings *******************************
    if (in_output_combined_video()) {
        std::cout << "video codec: " << in_codec() << std::endl;
	std::stringstream ss;
	ss << out_string.c_str() << "/" << "video.avi";
        std::string save_string = ss.str();
	      std::cout << "\nVideo File: " << save_string << std::endl;
        writer1=cvCreateVideoWriter(save_string.c_str(),0x00000000,//CV_FOURCC('P', 'I', 'M', '1'),//CV_FOURCC('D', 'I', 'V', 'X'), //CV_FOURCC_DEFAULT,
                            framerate,cvSize(width*number_of_cams,height),1);
        std::cout << "created video: \"video.avi\"" << std::endl;
    }
    

    // *********************************** Camera Setup ***********************************************

    for (std::map<unsigned, uint64_t>::iterator it = serial_num.begin(); it != serial_num.end(); it++) {
      std::pair<unsigned, uint64_t> input_pair = *it;
      captures.setGUID(it->first, it->second);
    }

    // Create Image containers
    std::vector<IplImage*> images(number_of_cams);
    std::vector<IplImage*> images_colour(number_of_cams);
    std::vector<vil_image_view<vxl_byte> > images_vxl(number_of_cams);
    
    // Individual setup parameters
    captures.setWidthAllCams(width); // Captured frame width
    captures.setHeightAllCams(height); // Captured frame height
    //captures.setFeature(0,"Shutter",500); // Length of shutter time on camera. (unitless value)
    //captures.setFeature(0,"Gamma",513); // Length of shutter time on camera. (unitless value)
    if (max_shutter() > 0)
      captures.setPtGreyFeatureAllCams("MaxShutter", max_shutter());
    if (max_shutter_abs() > 0)
      captures.setPtGreyFeatureAbsAllCams("MaxShutter", max_shutter_abs());
    captures.setColorModeAllCams(std::string(in_format())); // Set the colour mode
        
    //cout << "Setting common parameters..." << std::endl;
    // Common setup parameters
    captures.setIsoSpeed(in_speed()); // Firewire 400 or 800
    captures.setFrameRate(in_framerate()); // Input is a string. Allowed frames per second: 1.875, 3.75, 7.5, 15, 30, 60
    captures.setDMABufSize(bufs); // Size of dc1394 ring buffer
    captures.setCleanup(cleanup); // Attempt to clean up bus after unclean process exit
    captures.setSuppressWarnings(false); // Suppress frame drop warnings getting printed to screen
    captures.setDebug(false); // Output debug information to a log file
    //cout << "Opening captures..." << std::endl;
    
    // Set the cameras up on the firewire bus
    captures.open();
    //cout << "Finished Opening captures..." << std::endl;

    //printf("Get Images started\n");
    if (vflag) cvNamedWindow( "Output", 1 );

    int response = 0;
    for(int index = 0, count = 0; index <= num_frames;count++) {
        // Grab the frames quickly   
        //printf("Grabbing frames..\n");
        response = captures.grabFrames();
        //printf("Finished Grabbing frames..\n");
        if(!response){          
            printf("Could not grab a frame\n");
            quit(&captures, writers);
        }
        //printf("grabbed frames\n");
        for (int i = 0; i < images.size(); i++) {
            //OpenCV Images
            images[i] = captures.retrieveCvFrame(i);           // retrieve the captured frame
            if (!images.at(i)) {
                fprintf( stderr, "ERROR: OpenCV frame %d is null...\n" ,i);
                quit(&captures, writers);
            }
            // VXL Images
            images_vxl[i] = captures.retrieveVxlFrame(i);      // retrieve the captured frame
            if (!images_vxl.at(i)) {
                fprintf( stderr, "ERROR: VXL frame %d is null...\n" ,i);
                quit(&captures, writers);
            }
        }
        //printf("retrieved frames\n");
        fflush(stdout);
        //Convert images to the right colour
        CvSize imageSize = cvGetSize(images.at(0));
        for (int i = 0; i < images.size(); i++) {
            if (camera == VIDERE) {
                images_colour[i] = cvCreateImage(imageSize,IPL_DEPTH_8U,3);
                cvCvtColor(images[i],images_colour[i],CV_BayerGR2RGB);
            } 
            else {
                if (isColor) {
                    images_colour[i] = cvCloneImage(images[i]);
                } else {
                    images_colour[i] = cvCreateImage(imageSize,IPL_DEPTH_8U,3);
                    cvConvertImage(images[i],images_colour[i]);
                }
            }
        }
        //cout << "Making visualise" << std::endl;
        CvSize displayImageSize = cvSize(horiz_window_size, vert_window_size);
        IplImage* visualise;
        if (vflag) {
            if (isColor)
                visualise = cvCreateImage( cvSize(displayImageSize.width*number_of_cams, displayImageSize.height), 8, 3 );
            else
                visualise = cvCreateImage( cvSize(displayImageSize.width*number_of_cams, displayImageSize.height), 8, 3 );

            int corner_1_x = 0;
            int corner_1_y = 0;
            int corner_2_x = displayImageSize.width;
            int corner_2_y = displayImageSize.height;
            for (int i = 0; i < images_colour.size(); i++) {
                cvSetImageROI( visualise, cvRect( corner_1_x, corner_1_y, displayImageSize.width, displayImageSize.height ) );
                cvResize( images_colour[i], visualise );
                cvResetImageROI( visualise );
                corner_1_x = corner_2_x;
                corner_2_x += displayImageSize.width;
            }

            cvShowImage("Output", visualise);
            /*cvSetImageROI( visualise, cvRect( 0, 0, imageSize.width, imageSize.height ) );
            cvCopy( img1_colour, visualise );
            cvSetImageROI( visualise, cvRect( imageSize.width, 0, visualise->width, visualise->height ) );
            cvCopy( img2_colour, visualise );
            cvResetImageROI( visualise );
            cvShowImage("Output", visualise);*/
        }

        // Write to combined video if selected
        if (in_output_combined_video()) {
	    //std::cout << "Writing combined video frames.." << std::endl;
            IplImage* writer_frame = cvCreateImage( cvSize(imageSize.width*number_of_cams, imageSize.height), 8, 3 );
            std::vector<IplImage*> colour_frames(number_of_cams);
            for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
              if (images[cam_index]->nChannels == 1) {
                IplImage* colour_frame = cvCreateImage( cvSize(images.at(cam_index)->width, images.at(cam_index)->height), 8, 3 );
                cvtToBgr(images.at(cam_index), colour_frame, "GRAY8");
              }
              else {
                  colour_frames[cam_index] = images[cam_index];
              }
              cvSetImageROI( writer_frame, cvRect( imageSize.width*cam_index, 0, imageSize.width*(cam_index+1), writer_frame->height ) );
              cvCopyImage( colour_frames[cam_index], writer_frame);
            }
            cvResetImageROI( writer_frame );
            //std::cout << "Writing frame" << std::endl;
            cvWriteFrame(writer1,writer_frame);
            cvReleaseImage(&writer_frame);
            for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
              if (images[cam_index]->nChannels == 1) {
                cvReleaseImage(&colour_frames[cam_index]);
              } 
            }
	    if (!(dataCounter_ % ((int)framerate*5)))
	      std::cout << "grab_dv Info: Written " << dataCounter_ << " frames..." << std::endl;
        }
        
        // Keep a record of the time to ensure that it's not taking too long to write images
        struct timeval start_time;
        struct timeval end_time;
        gettimeofday(&start_time, NULL);
        double t1=start_time.tv_sec+(start_time.tv_usec/1000000.0);
        
        // Write to images if selected
        if (in_output_image()) {
            std::vector<IplImage*> grey_frames(number_of_cams);
            if (in_force_grey()) {
                for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
                    if (images[cam_index]->nChannels != 1) {
                        grey_frames[cam_index] = cvCreateImage( cvSize(images.at(cam_index)->width, images.at(cam_index)->height), 8, 1 );
		                cvtToGrey(images.at(cam_index), grey_frames[cam_index], std::string("RGB8"));
		            } else {
		                grey_frames[cam_index] = images[cam_index];
		            }
                    //std::cout << "Writing image" << std::endl;
                   std::stringstream ss;
                    ss << out_string.c_str() << "/" << "cam" << cam_index << "_image" << std::setw(5) << std::setfill('0') << dataCounter_ << "." << out_format();
                    //std::cout << "saving images to " << ss.str() << std::endl;
                    std::string save_string = ss.str();
                    cvSaveImage(save_string.c_str(), grey_frames.at(cam_index));
	            }
            } else {
                for (unsigned cam_index = 0; cam_index < number_of_cams; cam_index++) {
                    //std::cout << "Writing image" << std::endl;
                   std::stringstream ss;
                    ss << out_string.c_str() << "/" << "cam" << cam_index << "_image" << std::setw(5) << std::setfill('0') << dataCounter_ << "." << out_format();
                    //std::cout << "saving images to " << ss.str() << std::endl;
                    std::string save_string = ss.str();
		            cvSaveImage(save_string.c_str(), images.at(cam_index));
                }
            }
            for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
                if (images[cam_index]->nChannels == 1) {
                    cvReleaseImage(&grey_frames[cam_index]);
                } 
            }	  
	  
//            for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
//             std::stringstream ss;
//              ss << "cam" << cam_index << "_image" << std::setw(5) << std::setfill('0') << dataCounter_ << "." << out_format();
//              std::string save_string = ss.str();
//              //cvSaveImage(save_string.c_str(), images.at(cam_index));
//              vil_save(images_vxl.at(cam_index), save_string.c_str());
//            }
	    // Every 5 seconds worth of frames, tell the user that frames have been written.
	    if (!(dataCounter_ % ((int)framerate*5)))
	      std::cout << "grab_dv Info: Written " << dataCounter_ << " frames..." << std::endl;
        }
        // Write to videos if selected
        if (in_output_video()) {
            std::vector<IplImage*> grey_frames(number_of_cams);
	    if (in_force_grey()) {
	      for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
		  if (images[cam_index]->nChannels != 1) {
		      grey_frames[cam_index] = cvCreateImage( cvSize(images.at(cam_index)->width, images.at(cam_index)->height), 8, 1 );
		      cvtToGrey(images.at(cam_index), grey_frames[cam_index], std::string("RGB8"));
		  }
		  else {
		      grey_frames[cam_index] = images[cam_index];
		  }
		  cvWriteFrame(writers[cam_index],grey_frames.at(cam_index));
	      }
	    } else {
	      for (unsigned cam_index = 0; cam_index < number_of_cams; cam_index++) {
		      //std::cout << "Writing frame" << std::endl;
                if (in_single()) {
		            cvWriteFrame(writers[0],images.at(cam_index));
                } else {
                    cvWriteFrame(writers[cam_index],images.at(cam_index));
                }
	      }
	    }
            for (int cam_index = 0; cam_index < number_of_cams; cam_index++) {
              if (images[cam_index]->nChannels == 1) {
                cvReleaseImage(&grey_frames[cam_index]);
              } 
            }
	    // Every 5 seconds worth of frames, tell the user that frames have been written.
	    if (!(dataCounter_ % ((int)framerate*5)))
	      std::cout << "grab_dv Info: Written " << dataCounter_ << " frames..." << std::endl;
        }
        
        // Check the start and end times to make sure writing to disk didnt take too long
        gettimeofday(&end_time, NULL);
        double t2=end_time.tv_sec+(end_time.tv_usec/1000000.0);
        double result = (t2 - t1);
        if (result > 1.0/framerate)
            printf("Writing images took longer than one frame period: %.10lf milliseconds\n", result*1e3);
        
        //usleep(300000);
        
        if (vflag) {
            char c = cvWaitKey(5);
            if( c == 27 || !cvGetWindowHandle( "Output")) quit(&captures, writers);
            if( c == 112 ) cvWaitKey(500);
            if( c == 99 && in_output_key()) {
              for (int i = 0; i < images_colour.size(); i++) {
                 std::stringstream filename;
                  filename << out_string.c_str() << "/" << "cam" << i <<"_image" << std::setw(5) << std::setfill('0') << dataCounter_ << "." << out_format();
                  std::string filename_string = filename.str();
                  cvSaveImage(filename_string.c_str(), images_colour[i]);
                  std::cout << "Wrote file: " << filename_string << std::endl;
              }
           }
        }
        
	
	// Increment the counter
        dataCounter_++;
        
        // Release data
        if (vflag) cvReleaseImage(&visualise);
        for (int i = 0; i < images_colour.size(); i++) {
            cvReleaseImage(&images_colour[i]);
            cvReleaseImage(&images[i]);
        }
        // Increment frame counter
        if (num_frames > 0) index++;

    }
    quit(&captures, writers);
}

